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Abstract 

Navigation function (NF) is widely used for mo¬ 
tion planning; such a function is bounded, analytic, 
and guarantees convergence due to its Morse nature, 
while having a single minimum point at the target. 
This results in a safe path to the target. Origi¬ 
nally, NF was developed for deterministic scenarios 
where the positions of the robot and the obstacles 
are known. Here we extend the concept of NF for 
static stochastic scenarios. 

We assume that the robot, the obstacles and the 
workspace geometries are known discs, while their 
positions are random variables. We define a Probabil¬ 
ity NF (PNF) by introducing an additional permitted 
collision probability^ which limits the risks (to a set 
value) during robot motion. 

We apply the Minkowski sum for the continuous case 
when considering the geometries with the Probabil¬ 
ity Density Functions (PDF). The PDF for collision is 
therefore the normalized convolution of the robot ge¬ 
ometry, the obstacles geometries and their locations’ 
PDFs. 

We give an approximation for the permitted prob¬ 
ability for collision. We then formulate an explicit 
function and prove that it is indeed a PNF. Finally, 
we exemplify our algorithm performances, and com¬ 
pare its results with a conventional NF algorithm. 

1 Introduction 

Motion planning for mobile robots has been exten¬ 
sively studied over the last three decades. Ideally 
one can assume that properties describing the robot 
movement, the environment and the obstacles are 


perfectly known. However, these parameters are of¬ 
ten affected by substantial random factors (referred 
to as random variables) due to measurement noises 
and physical process. In the presence of uncertain¬ 
ties, even simple notions become non-conclusive, see 
for example the prediction of collisions between mov¬ 
ing objects considered in m- Researchers have 
studied algorithms to deal with process randomality: 
Lazanas and Latombe m use a back-projection al¬ 
gorithm to define areas where sensing may be consid¬ 
ered accurate and these areas are added together to 
form ’’safe zones.” A similar line of action is to max¬ 
imize certainty by approaching known landmarks. A 
modification of A * algorithm, introduced in [12, is 
constituted by adding a fourth dimension to the ge¬ 
ometry. This additional dimension corresponds to 
the uncertainties, forming a mathematical structure 
named ’’towers of uncertainties, ” which results in a 
path with a lower uncertainty. Pepy and Lambert 
|20| define a configuration with the additional un¬ 
certainty data {ax,crY,crz,0,4>)i and then introduce 
a safe-RRT algorithm for a solution. Their exper¬ 
imental results show paths which indeed form safe 
trees that follow the walls in order to reduce the un¬ 
certainties. 

A different viewpoint which guarantees convergence 
is to follow the robust control approach. This can be 
applied for path planning in the sensor’s image space 
Hi!, or be used to stabilize uncertain non-linear sys¬ 
tems along nominal paths |2B], [IS]- However, note 
that these methods require well established model 
equations. 

A third approach is to replace the obstacles’ loca¬ 
tions with the computed probability for collision, for 
example as done for moving obstacles by Fulgenzi [5] . 
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This paper presents the problem of motion planning 
for a static uncertain environment by taking into con¬ 
sideration both the geometry and the location proba¬ 
bilities functions without inflating the ambient space 
dimension while guaranteeing convergence. This is 
done by extending the well known deterministic Nav¬ 
igation Function (NF). 

NF [To] is one of the best known method due to its 
mathematical elegance and simplicity. A NF is a con¬ 
tinuous smooth function with zero value at the target 
point and a unity value on the boundaries of the en¬ 
vironment and the obstacles. In order to ensure a 
solution, the NF critical points are non-degenerated 
(i.e. there are no plateau areas in which the gradient 
of the NF vanishes). Other concepts of NF’s have 
been proposed, for example Lavalle and Konkimalla 
m numerically solve a discrete differential equation 
to obtain a NF which simultaneously yields both the 
geometric path and the control signal, while other 
methods provide these in a two-step procedure. An¬ 
other important advantage of the NF algorithm is its 
asymptotic convergence property. Some researchers 
attempted to apply the classical NF method to un¬ 
certain environments: Palejiya and Tanner m apply 
the NF when certain switching conditions are met. 
Loizou et al. m use a NF in a portion of a config¬ 
uration space with the convergence property verified 
through computer simulations. To the best of our 
knowledge, no attempt has yet been made to modify 
the NF to fit stochastic scenarios without inflating 
the ambient space dimension. In this paper we shall 
extend the concept of NF to static stochastic envi¬ 
ronments and analytically prove its convergence. 

1.1 Problem formulation 

Let C be a robot configuration space. Assume that 
C is a subset of a smooth manifold which is K". We 
denote the robot location by S C and the f-th ob¬ 
stacle fixed location by G C, where O denotes 

the set of obstacles. Here, the location refers to the 
center of the object. Since deterministic knowledge 
about the x^jX^go, is not always available, we use 
a set of probability density functions (PDF). In this 
paper we assume the distributions are Gaussian and 
denote a PDF by p(x), where x ~ A/'(x, E). Note 


that while the locations are random variables, the ge¬ 
ometries of the robot and the obstacles are perfectly 
known. We assume that x^, x^go are estimated using 
a nonlinear filter, which is a source for uncertainties. 
In this paper we assume that all locations are esti¬ 
mated using an external tracker so the uncertainties 
of the locations of the robot, and the obstacles are 
independent. Our main problem is formulated as fol¬ 
lows: 

Given a static environment with a proba¬ 
bilistic density function of the robot’s and 
the obstacles’ locations, that characterizes 
the uncertainties of their localizations, de¬ 
termine whether convergence of motion 
planning to the target’s configuration (qii) 
is guaranteed for a given allowable proba¬ 
bility for collision (A), and if so, generate 
a path that reduces the probability for col¬ 
lisions. 

Note that we seek a smooth connected path tt : 
[0,1] —?► C, (in what follows we shall track the steep¬ 
est descent curve of a smooth function : C —>■ K). 
Here, A indicates the highest allowable probability 
for collision (see for example [18] and (2T]). It is ex¬ 
pected that in some scenarios the robot would follow 
a shorter path at the expense of the collision proba¬ 
bility. Thus, A limits the probability for collision to 
a user-determined value. 

Next, assume the obstacles do not intersect even 
when taking a dilated radius i?A around each ob¬ 
stacle, which encloses A probability for collision (the 
curve in Eq{T^. One can apply a deterministic NF 
(where all obstacles are dilated by a constant radius 
i?A, c.f. [15]) to the problem. However, the proposed 
PNF considers the uncertainty of each obstacle as il¬ 
lustrated in Fig[2 In this figure the uncertainty of 
the right obstacle’s location is larger than the uncer¬ 
tainty of the left obstacle’s location. The PNF con¬ 
siders these different uncertainties, and therefore, we 
anticipate that it will provide a safer path compared 
with the deterministic NF approach for uncertainties. 

While the theoretical scope of the paper is valid for 
all dimensions, the mapping of a spherical obstacle 
from the workspace to the C may be complex. How¬ 
ever, such a description is suitable for a large set of 
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Figure 1: A comparison between paths in the PNF 
(a) and in the NF with a Ra inflated radius (b).S' 
represents the starting point and T the target and 
the bold lines indicates the. The light solid discs 
represent the obstacles and the dark disc the robot. 
Here, the STD of the right obstacle is larger than 
that of the left. While the NF considers the distance 
from the inflated boundaries, the PNF considers the 
probability for collision. 


practical scenarios: (1) spatial mobile robots, such 
as unmanned aerial vehicles (2) n-dimensional se¬ 
rial robots with point obstacles, located ’’far enough” 
from the base joint [3], (3) spider-like planar robots 
with point obstacles near the end-effector [24] . 

2 A Probability density func¬ 
tion for collision 

We apply a modified NF in order to incorporate 
the position uncertainty of the robot and the obsta¬ 
cles, and call this function the Probability Navigation 
Function (PNF) or the Stochastic Navigation Func¬ 
tion (SNF). The PNF describes the probability for 
the robot to collide with an obstacle at a given point, 
as well as the distance to the target position. In or¬ 
der for the algorithm to be as realistic as possible, 
the robot and the obstacles possess finite disc shapes 
(rather than being a point mass). 

The shapes of the robot and the obstacles are de¬ 
scribed by a probability map, and the path is then 
generated as the PNF gradient. A common tech¬ 
nique used when dealing with motion planning prob¬ 
lems (see |7| §10 for extended discussion), is to define 
the free configuration space Cfree (he. a subset of 
C C R" where the robot can travel without colliding 


with obstacles, excluding the boundary). A prevalent 
method is to define Cfree as the complement of Cobs, 
the union of the Minkowski sums of the robot with 
the set of obstacles. Intuitively, the obstacles in the 
C space are expended by the robot’s volume, while 
the robot is taken as a point mass. The set of vectors 
defining the robot’s geometry, are measured from its 
center of mass to any point on the robot body and 
are denoted by A. The set of vectors defining the 
geometry of all obstacles measured from the origin 
to their body points are donated by B. Thus, we can 
write: 


Cobs=B*{-A) = {b-a\aeA,beB} (1) 

(we use * to denote both Minkowski sum and Con¬ 
volution operation). Note that in order to measure 
the distance of a point inside the robot from a point 
inside the obstacle, one should first rotate the robot 
by 180° (the minus sign in Eq. [^. The sets A,B 
are sub spaces of C making B * (—A) large. One way 
to overcome this is to confine calculations to an in¬ 
termediate time step (i.e. A,BcWC R"). Figj^ 
demonstrate this process. 

We shall now incorporate the geometries of the robot 



Figure 2: The Minkowski sum of (a) a pentagon 
shaped robot and a trapezoid shaped obstacle, (b) 
The rotation of the robot by 180°. Here, the shortest 
distance from the low left corner of the obstacle to 
the robot edge is the same as the shortest distance 
from the center of the robot to the edge of the ob¬ 
stacle after summation, (c) The resulting point mass 
robot and the inflated obstacle. 


and the obstacles together with their location prob¬ 


abilities over three stages (which correspond to (2.1 

p)^and p3|): 
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2.1 Convolution of the obstacle’s 
geometry with the robot’s 
geometry. 

Let us define the disc geometry function as: 


D(x,Xc,r) 


A 


1; ||x-Xc||<r 

0; otherwise 


Accordingly, define rob (x) = D{x,Xr,rr) as the 
robot’s geometry. Similarly, define the i-th obstacle’s 
geometry as obsi (x) = D{x,il^,ro)- The Minkowski 
sum of both functions is denoted by: 


obsi (x) = rob (x) * obs (x) = D{x, x*, R) 


where x is a point in C, and x^ the estimated location 
of the obstacle’s center. The estimated location of the 
robot is Xr and R is the radii sum of the robot r^. and 
the obstacle Xq. The new robot geometry function is 
now: rob{x) = i5(x —x^), here <5 stands for Dirac’s 
delta function. 


2.2 Convolution of Gaussian func¬ 
tions. 


2.3 Convolution of probability density 
function and geometry functions. 

Note that in Eq. the robot and the obstacles are 
represented by a point- mass. To extend this we shall 
now investigate the probability for a collision of a disc 
shaped obstacle with a point mass robot (as is often 
done in motion planning problems). 

The location of any point v of the obstacle relative 
to a fixed point on the obstacle (e.g. its center of 
mass) is a deterministic value. Therefore the loca¬ 
tion can be defined as a constant random variable 
(see [5] §5) by the probability function: pv (x|x*) = 
(5 (x — (xq -I- v) ). Note that the i-th obstacle cen¬ 
ter location x), measured in a global coordinate sys¬ 
tem, and V which is measured in a local coordinates 
system, are independent. The convolution of these 
functions, which yields the probability distribution 
function for an infinitely small portion v S obSi{x), 
is: 

Pv (x) = Pv (x|x(,) * /i(x) 

Applying (cf. pg. 53) yields: 

Pv (x) = /i (x - (x* -F v)) 


To implement the above to a stochastic scenario, let 
us first consider point-mass obstacles and a point- 
mass robot with given probability density functions 
embedded in K" for arbitrary configuration space di¬ 
mension n. Eq. defines a map /^ : C —>■ K. That 
is, the Minkowski sum is replaced by a (continuous) 
convolution of the probability functions. Thus, 

/,(x) = p (xlx*, E*) *p (x|0, S^) (2) 


which is the PDF for a cell point v of the obstacle to 
be at X. 

Recall that the robot center is at x, we would like 
to avoid collision with any of the obstacle’s points, so 
the density function for such a collision is given by 
the integral: 


Ptot (x, R) 



obsi (v, R) /j(x - v)dv 


following [27], Eq. [^results in the probability func¬ 
tion of the Ath obstacle location: 

/^(x) = . 

|E), + E,|5 

with expectation x), and covariance E^ = E* -|- E^. 
We denote the distributions for the location of the 
robot and the locations of the obstacles by: 

Xr ^ A/” (Xr, 0) = 5 (x — Xr) (3) 

x(,^AA(x(„(E, + E(,)) =p(x|x*,E,) (4) 


where A is a normalizing scale factor, selected such 
that Jptot{x)dx = 1 (see (2.6). 

Note that ptot concerns on y one obstacle. Since mul¬ 
tiple obstacles are involved, we shall combine all the 
corresponding functions ptot when defining the prob¬ 
ability NF (see (J^. 

For further clarification, note that the probability for 
a collision of the robot with an obstacle estimated to 
be at Xo is given by the integral: 

Pr(|lxr - Xoll < R) = J Ptot{^r,R) 
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One can think of the convolution operator as locating 
the i-th obstacle at the origin so x* = 0, while moving 
fi around. This means that v can be considered here 
to be in either a global or a local coordinates system. 
Therefore it is easy to see that: 

Ptot (x, R) = ^obsi (x, R) * fi (x) (5) 

2.4 Convolution of n dimensional disk 
with Gaussian distribution. 


which may be rewritten as: 


C(g) = 


-g 

e 2 a 


2 R 


( 271 ( 7 ) • 


r\_nl2n—l 
I'K ' T e 2a 


V^r((n-l)/2) 


e " sin" ^ ((/)i) dcpi 


dr 


9.6.10 , 9.6.18): 

(with a diagonal covariance of the form: S = al) and 

a disc D{r, R) : ^ 

C{r)=D{r,R)*G{r) (6) C (g) = (2a)-§e^ £ ~ 

k—0 

For an arbitrary Gaussian, E can be taken as a diag¬ 
onal matrix with all its entries equal to the maximal 
eigenvalue of the covariance matrix. 

Assume the disc is centered at the origin and the 
Gaussian is at g S K": 


R-‘ 


r{k + n/2) 




k-\- ^ — 1 _ z 

^ e 2 ' 


dr" 


We now focus on a specific case applying the analysis 
introduced in (2.3 above. Consider Eq. [^- a convo¬ 
lution of a normal distribution G(r), where r G M" 


where r = ||r|| and g = ||g||. 
Following Abramowitz m Eqs. 


[22] considers the convolution of a disk centered about 
the origin in K" with a Gaussian centered about an 
arbitrary point. We now generalize Plesser’s results, 
for arbitrary Euclidean ambient space and arbitrary 
(7. Eq. [^may be formulated as: 

C'(g) = J G{r-g)D{r,R) dr 


b 

Recall that P{a,b) = / e~^x°‘~^dx is the Nor- 

malized Incomplete Lower Gamma Function. Rear¬ 
ranging terms results with the equality: 

r® / = Tw /^ (»• f 

0 0 


The Jacobian for the polar form of the above is 

n—2 

J = a sin^ (see [2|, Pg. 65-66) and 

k=l 

thus: 


Finally, for an n-dimensional disk-shaped obstacles 
distributed normally, Eq. [^becomes: 


C(g) = 


27rr' 


n —1 


{2Tra) ■ 


n—3 


It, -O p 

P sin’^ {(j)n-l-k) d(j)n-l-k 
k=l{ 

2 " sm {(fi) dcfidr 


Plot (x, i?, cr) = 



m—0 



where x is the location vector. 


f n R^ 

r+2’^ 

(7) 
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2.5 The Gradient and the Hessian of 

Ptot' 

We now calculate the Gradient and Hessian of[7l 
dptot{fl,R,cr) 5||q||| _ 


where ^ = |r| is the distance from the origin and A is 
the normalization factor, which by expansion yields: 


1 


°° „2m 

^ g 


E 


Vptot(q,i?,cr) = 


|q|li 


dq 


= 2qe 


E 


|2(m-l) 


(2cr)""(m- 1)!'^ 2’ 2a) 


n R^\ 


^ A J ^ ^ (2cr)""m! 

m—0 ^ ' 

Again, using [5] results in: 

1 7r("-i)/2 


, nR^\ 


-A = 


Ar((n-l)/2) 


P ( m + 


m—Q 


n R^\ 1 

2’^y (2CT)™m!' 


- H (2») 


-m-ll|q|| 


m—0 


mi 


n R^ 
P { m —, — 
' 2 ’ 2cr 


Ri 


(9 


2\m+f-l ^ 


Following Gautschi [5] we know that: P(a + l,a:) — 
P{a,x) = — and since the modified 

Bessel function can be written as: la {x) = 


e / s\tP~( j)d(l)dg‘^ 

0 0 

Denoting the Double Factorial by A!! results in the 
following equality: 


E 

m—0 


m!r(m+a+l) 


(!) 


2m+a 


TT 

l{n) = j siiP4>d(j) = 


{n — 1)!! j TT, n mod 2 = 0 


nil 


2, n mod 2 = 1 


Vptotiq,R,(j) = 
The Hessian is: 


-2qP"e 


(2a) 


1+1 


|q|l R 


'^Ptot{q,R,cr) = e 2, 


iiq|i^+«^ qq^P" 


• li 


|q||P\ R 

) llq|l ^ 


2 2 (7 2 +2 

q|| R 


( 8 ) 


(9) 


and the value of A can be simplified due to Fubini’s 
theorem: 

r f f P”7r"/^ 

A = / D{r) * G(v)dY = / D{r)dr / G{r)dr = 


r(t + i) 


Finally Eq. |ll| may be written as: 
r(n/2 + l) 


A = 


2.6 Minimal permitted collision prob¬ 
ability 

In order to ensure a reasonably safe movement, we 
limit the maximal collision probability to a predefined 
value A. In other words, we are interested in a closed 
curve lb in C C R” such that: 


^ j obsi (x, R) * fi (x)dx = A (10) 


We pursue a safety distance Pa from obsi that will 
ensure probability for collision of at most 1 — A: 

A=jjG{Od-i (11) 


V^P"r((n-l)/2) 
n 1 
2’2a J 


^ i f n R^\ f n R\ 

■ / —r-P m + TTi 7 m + ttj — 

2—^ 777! \ 2 2rr / \ 2 2rr 

m—0 ^ 


( 12 ) 


where ^(a^b) = J e ^dt , is the Lower Incom- 
0 


plete Gamma Function. Eq. 12 can be approximately 
solved for Pa- pa can be calculated as ptot{x) for 
(all) X G which is a circle of radius Pa through the 
obstacle location (i.e. pA = Ptot (Pa, P, o') see also 

Eq.[7|. 

3 Probability Navigation Func¬ 
tion 

This section presents the approach for generating mo¬ 
tion planning in uncertain environments. The follow- 
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ing discussion is an extension of the deterministic NF 
suggested by Rimon and Koditschek nni. Denoting 
the target position by the NF is defined at a point 
g S C as: 




il 


bf + P (q)]' 


(13) 


where fc is a predefined constant which ensures the 
Morse nature of the function (for both the determin¬ 
istic and probabilistic NFs). Recall that a real-valued 
smooth function on a differentiable manifold is called 
Morse if all its critical points are non-degenerated; 
k will be discussed in Section 7d(q) = llq- qdiP 
and P (q) is: 

No 

/3 (q) = n (‘i) (14) 

i^O 


where: (q) = 


-||q-qolP + Po ;* = o 
l|q-q»lP-p? D>o 
Here qo defines the center of the permissible area, 
considered as the coordinates’ origin, while for all 
i G O, is the center of the f-th obstacle. 

The numerator of Eq. [^is defined in such a way that 
the robot is attracted to the target position, while the 
denominator ensures obstacle avoidance. 

Considering a stochastic scenario, we would like to 
minimize the probability for a collision while main¬ 
taining the shortest path to the target. In the deter¬ 
ministic scenario, Pi (q) is a function of the distance 
between q and the obstacle’s boundary. Our goal is 
to replace Pi by a function that is based on the prob¬ 
ability for collision at location q. We set a threshold 
value for collision probability by replacing the obsta¬ 
cles’ geometric edge by the edges - discussed above 
(see Eq. 10). 

We then modify P to fit an uncertain environment. 
In a deterministic scenario, Lpx decreases the distance 
to the target position while avoiding the obstacles. In 
a probabilistic scenario the probability for collision 
would be limited by a predetermined value- A. 

In order to do so, we replace the original p with 
the probabilistic value Pi(q) - the probability den¬ 
sity function at g (discussed in Section 2.3). This 
equals ptot(q — qi) (see Eq. computed for the f-th 


obstacle {i G O) and for the workspace boundary: 

/^*(q) =PA,-Pz (q) 

/3o(q) = -PAo +Po (q) 

where, pAi = Ptot (Rai, Ri, cfi) Thus, Pi and P vanish 
on the extended boundaries of each obstacle defined 
by i?A , i.e. where the probability for collision is A 
(see Eig§. 

Note that po and paq refer to the external boundary, 
computed based on the probability density function 
of the robot, and also that paq is computed as in Eq. 
[T^ replacing A with I — A. 

4 Is (/? a Probabilistic Naviga¬ 
tion Function? 

We start by defining a NE in the context of pH] : 
Definition: A map is said to be a navigation func¬ 
tion if it satisfies the following conditions: 

1. It is analytic in all q G Cfree] 

2. It is polar throughout C, with single minimum 
at qrf G Cf 

ree ; 

3. It is morse on Cfree, 

4. It is admissible on Cfree- 

We now extend the above definition to a stochastic 
scenario and prove that such a function is indeed a 
probabilistic NF: 

Definition: A map p is said to be a probabilistic 
navigation function (PNF) if it satisfies the following 
conditions: 

1. It is a NF. 

2. The probability for collision is bounded by a pre¬ 
defined probability A. 

Note that as a consequence of the above, following 
Vp minimizes the probability for collision (subject to 
decreasing the distance to the target). The NF is the 
composition: 

If = ad o a o ip 
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where: ad{x) = ; a{x) = and ^ ^ 

and o is the composition operator. In this paper we 
change only ip. According to proposition 2.7 in m 
it suffices to verify the first condition- (1) only for cp 
(note that the forth requirement directly follows from 
the definitions). 

We shall now prove that (p constitutes a NF. In 



Figure 3: The partition of the configuration space: 
(A) is the region which extends away from the obsta- 
cfes such that (3i > e and stretches to the configura¬ 
tion space boundary (B) is the region which extends 
from the obstacles’ boundaries, and away from it up 
to Pi < £o: (C) is the region which extends up to 
/3o < El away from the configuration space boundary. 
i?A indicates the safety obstacle radius, see Subsec¬ 


tion 2.6 


Propositionj^we will prove that p attains a minimum 
value at the destination qd- In order for our motion 
planning scheme to converge, p must not have crit¬ 
ical points on dCfree (he. points where the gradient 
vanishes), which we shall prove in Proposition]^ that 
results in the interior of Cfree- 

For convergence we require that all critical points 
in Cfree sxe non-degenerated. We refer to this re¬ 
gion as ’’near the i-th obstacle” and denote it by 
B^ie) = {q| 0 < /3*(q) < e}. Since the obsta¬ 
cles do not intersect, there exist e > 0 such that 
/3i(q) n/3i(q) = 0 for alii j G O and /3j(q) n q^ = 0 
for all i G O. In other words, we need to prove that: 


has no non-degenerate critical points in either regions 
(indicated by the three components). Propositions]^ 
and ]^ respectively prove that the first and second 
regions have no critical points, while Proposition ]^ 
proves that all critical points near the obstacles are 
not local minimum points. In Proposition ]^ we con¬ 
clude that (/? is a Morse function by showing that the 
function is non-degenerate near the obstacles. 

Proposition 1 The destination region located at q^ 
is a local minimum of p. 

Proof 1 This is identical to the proof of Proposition 
3.2 in fW^ . 


For the following discussion, we denote: pi = 

No 

n Pj 

j=0,jpi 

Proposition 2 All the critical points of p are in the 
interior of Cfree ■ 

Proof 2 We focus our attention on some point q' G 
dCfree- Obviously Pi = Q for a certain i G O, and 
Pj > 0 for the rest j ^ i. Differentiating yields: 

Vp (q') = ^ (^V7d - ^7^" (fc7d-'V7d + V/3)^ 

= - a 0 

kid 

which proves the proposition since: 

^Pi (q') = -'^Ptot (q' - q*, Ri, ^i) A 0 


As k increases, the critical points of p approach those 
of ‘jd- We show this by proving that there are no 
critical points far away from the obstacles: 


No 

Cfree = {q|A(q) > e, Vi e O} U Bq (e) u IJ A (e) 

i=l 


Proposition 3 For every e > 0 there exist N{e) G M 
such that for all k > N(e), p has no critical points 
in {q|A(q) > E,Vi S O}. 






Proof 3 Note that if tp has no critical points at a 
given region, neither will ip. Thus we prove the propo¬ 
sition for ip. 

A critical point satisfies: 

P 

so: 

= 7dV/3 (16) 

Taking the magnitude of Eq. \1(\ yields: 
fc/3 II V 7 d|| = 7 d II V/3|| To avoid a critical point 
we require: k > 

No 

Since, V/3 = X) ||V 7 d|| = and 

i=0 

-^ = Pi > s, the parameter k must comply with the 
following constraint: 

k> ^max{7^}^max{||V/3,||} = lV(e) (17) 
with max{ 7 d(q)} = i?o + ||qd||- 


Let us denote As = ^{A A^) - the symmetric part 

of the matrix A, so we can write: 

VV(q) = ^(fc/3V^7.+ (l-^)- 

(/3fVAV^f + 2 AA(vAVA^)^ + - 

-7d [PN7^P^ + 2{vpiNP^^)^+P^N^P^)) 

Note that VfAis = iFVfi = 0, and = 2/. Tak¬ 
ing the quadratic form of if by an arbitrary orthogonal 
vector to NPi: v = we can write: 

xAv'^ip (q) V = 'ydPiV^V'^PiV-P 

(^kpi + (i - P^yp^ypI + id^^P^j V 

(19) 

It is hard to conclude whether the second component 
is positive or not. But note that the Hessian of Pi 
(see Eq. 


Proposition 4 There exists an Sq such that ip has 
no local minimum in the set Bi{e),i € O (near the 
obstacles) for e < Eq: 

Proof 4 The NE must ’’flow” around the obstacles. 
We therefore, show that at least one eigenvalue of 
N'^ip is negative by calculating the projection onto the 
direction perpendicular to the gradient of Pi at q. 
Consider a critical point q^ £ Bi (e). The Hessian of 
ip is: 

V^T(ci) = ^(PV^l^d-ld^"P) = 

k—2 

= ^ i^P + {k-l) V7dV7j) - llV^P) 


V'^Pi = -V^ptot(q- q*,i?i,cri) 

is negative definite since, Io(x) > Ii(x) \/x and ||q — 
qi|| > Ri yq G Bi{e). Additionally, both jd and Pi 
are positive, therefore the second term is negative. 

To ensure that Eq. [7^ is negative we can bound Pi 
with e by: 

min {"fdPiv'^N'^Piv} 

A _ qeBi(7 _(_ 

“ max {yx (2kpi + (l - ^) PiNpNPj + -fdV^Pi) «} 

q6Bi(E) 

See Lemmas and in the Appendix for explicit ex¬ 
pressions of the extermal terms. 


Taking the tensor product of both sides of Eq. 
yields: 


{kpfy/jd^^J = ^jy/py/p^ 


Proposition 5 If k ^ then there exists an 

ei > 0 such that ip has no critical points near the 
workspace boundary, as long as e < Ei. 


So, the Hessian of ip becomes: 

V2^(q) = ^ {kpyj^^d+^^jyjpNP^ 


Proof 5 

7iV2/3j 

(18) V(^V7d = 


The inner product: 

k k 

^ Akp - V/3V7d) >Po^ {ikPo - V^oV7d) 
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according to Eq. 0 /3o^ - V^oV7d) > 0. 

To estimate the second term, define ei as the prob¬ 
ability for a robot located at to collide with the 
workspace boundary. 

^1 — PAq Ptoti^d^ -htot ^r) 

/3o is restricted by: /3o = PAq - Ptot(q, -Rq, CTr) < £i 
This is valid since all points in jB(eo) are 
closer to the boundary than qu- V/3o points away 
from the destination qa at any point q in B^Eq) 
since Vf)o =—Vptot Ro,o'r) (see Eq. and 

Vjd = 2||q - qdll > 0, so V7dV/3o < 0. This com¬ 
pletes the proof. 


We showed that near the obstacles there may be crit¬ 
ical points of (p. We also proved that such points will 
have a negative gradient component directed tangen¬ 
tially to the obstacles. Yet, in order for (,5 to be a NF 
we need to show that it is a Morse function. 


Since q € Biie), and assuming that k > 2 it can be 
rearranged as: 

Eor the first term to be positive we require: 

inin {llVAf} 

, ggSde) _ A ' 

^ ~ 4 max {|V2/3i|} 

and a sufficient condition for the second term to be 
positive we require: 

^2 ^ AIIVAIP ^ min{y/^||V/3i||} ^ 

^ - 4|u'rV2/3,{i| - 27max{|V2/3i|} ^ 

See Lemmas [7] and in the Appendix for explicit 
expressions for the extremal terms. By restricting 
the distance of q to the obstacles such that /3(q) < 
min{e', e”}, we guarantee that p is a Morse function. 


Proposition Q p is a Morse function. 

Proof 6 We would like to prove that the component 
of the gradient of p in the radial direction to the ob¬ 
stacle is positive. This way Vp will not have any 
degenerate direction as required. 

Substituting^^ into Eq. and multiplying both sides 
of the equation by: v = ||vf% becomes: 


~Tv72 - ~ 

Algebraic manipulations lead to (compare with [Prop. 

3.9, mi): 


fi¬ 


ll 


-v^ u) 


Finally, in order for to be a NF in all C, we require 
that E = mm{eo; £i,£' , £"} which is also required for 
determining k (constrained hy k > N(e)). 

5 Some Examples 

This section presents examples of the PNF motion 
planning using MATLAB. We set the world’s radius 
to 45 units length. Fig. |0a depicts a stochastic sce¬ 
nario where the obstacles radii from the top right c.w. 
are 4,4 and 2, while the locations’ STDs are 6,5, and 
4 respectively. The robot radius is 3 and its location 
STD is 4. A is chosen as 0.1 while k is chosen to be 5 
empirically (the larger k is, the closer the PNF allows 
the robot to approach the obstacles). Fig. |^b depicts 
a scenario where the obstacles have the same geom¬ 
etry, while the STDs are 30, 5, and 4 respectively, k 
is again chosen as 5 , and A remains 0.1. In Figj^c 
we use the same geometries and the same standard 
deviations as in Fig|0a, but k is chosen as 2 and A 
remains the same, (observe that the PNF seems far¬ 
ther to the obstacles). 

As for a different selections of A, in Figj^d A = 0.6 
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and the path length is 77 units, where in Fig|^e 
A = 0.8 which results in path length of 84 units. 
Finally, Figj^f depicts a simulation with two slightly 
different initial configurations {Si and S 2 ) which re¬ 
sults with bifurcation. Moreover, note that poorly 
chosen constant k (2) results in undesirable local min¬ 
ima located at two points. In this case N{e) is large 
since the obstacles are close to each other, resulting 
in a small e (see Prop. [^. Recall that k > N{e) and 
thus k should set larger avoid bifurcation. 


A 

Method 

Path Length 

STD 

Failure [%] 

0.9 

PNF 

NF 

46.74 

38.95 

28.38 

10.87 

1.53 

8.56 

0.67 

PNF 

NF 

44.09 

37.20 

25.16 

7.48 

2.64 

9.02 


Table 1: Performance comparison of the PNF and NF 
with different As. Numbers are the average of 200 differ¬ 
ent simulations with the same distributions and geome¬ 
tries. Failure refers to an obstacle-robot collision. 


(a) (b) (c) 



Figure 4: Implementation of the PNF for sphere- 
world motion planning problem. The target is 
marked as The bold line indicates the path from 
the initial point to the target (or to local minimum 
at /). The dark solid discs are the obstacles and the 
light solid disc the robot 


Table 1 compares the performance of a PNF with 
a path planning generated by a traditional NF where 
we considered an inflated geometry of the obstacles 
with radii equivalent to A (e.g. for A = 0.9 the radius 
addition is 1.67 STDs). Note that prior to construct¬ 
ing the NF, we performed a convolution of the robot’s 
geometry with the geometry of the obstacles (as per¬ 
formed for the PNF). This was essential in order to 
compare the resulting paths from the two functions. 


6 Summary 

We defined a probabilistic navigation function, such 
that following its gradient produces a path that de¬ 
creases the probability for collision with the obstacles 
and converges to the target point. In order to pro¬ 
vide a ” safe” motion path, we included an additional 
requirement for a maximal permitted probability for 
collision. 

We have introduced such a function defined on 
C C K" and showed that p is indeed a probabilistic 
NF. 

We proved that the PNF converges for all stochastic 
scenarios. In order for the analysis to be as analytic 
as possible we assumed disc-shaped elements and Ra¬ 
dial Gaussian distributions to model the uncertain¬ 
ties. That is, given a disc-shaped robot and disc¬ 
shaped obstacles with given uncertainties in their lo¬ 
cations (in a disc-shaped world), we have shown how 
to construct p which will safely transverse to the tar¬ 
get. Note that the discussion in this paper can be 
generalized to star-shaped worlds as well, in exactly 
the same manner as used in [23) . 

We have demonstrated our algorithm on various sce¬ 
narios, showing how the selection of k affects the re¬ 
sulting paths. We also provided experimental results 
showing the effect of the extent of uncertainty on the 
path. Lastly, we compared the PNF to a simple NF 
showing that the resulting path from the PNF is safer 
(but naturally longer). 

The PNF can be further extended to algorithmically 
include the robot’s dynamics see for example [9]. In 
future work we intend to apply the PNF to the more 
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general problem of stochastic-dynamic environment 
and to include generalized Gaussian distributions and 
geometries. The authors also wish to continue inves¬ 
tigating a version where there is no assumption for 
pairwise obstacle distances- this is done by compos¬ 
ing a second function (similar to that introduced in 
[23) that can handle the case of non-spherical unified 
obstacles. 
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Appendix 

Now, we shall prove some of the bounding e’s we used in 
SectionlH 

_ 1 

Lemma 1 max{||V/3i||} < and, 

maxlllV^dill} 

Proof 7 Throughout the paper jj jj denoted the Euclidean 
norm. Here we use || jjp to indicate the general p-norm 
(e.g. II II 2 = II II ). - Recall that pi is based on the convo¬ 
lution of the disc with a Gaussian. Thus as a consequence 
of Young’s inequality ||V/5i||2 can he written as: 

IIV {D{r, Ri) * G(r, a,)) ||2 = |lD(r, R,) * VG(r, aOlh 

Again using Young’s inequality, this amounts to: 

||D(r, AO * VG(r,a0||2 < C2,i||D(r, AOlO * ||VG(r,a0||i 

where C2,i < 1. Since D{r, Ri) is a disc with a unit height 
we have: 

_ 1 

max{||Vdi||} < max{||VG(r,cr0||i} = ^ 

V 27ro-/ 

Using the same logic: 

max{||V^/3i||} < max{||V^G(r,(j0||i} = 

V TT af 

■ 

Lemma 2 

max {di} = TT PAj - Ptot{\\qj - cii\\-\-R^,Rj,aj) 

aGBiis) 

je{0-i} 

min = n P^j -Ptot{\\cij - qi|| - Re,Rj,aj) 

je{0-i} 

Proof 8 Since Pi = H Pj we have 
j£{0-i} 

max {Pj} =pAi -ptot{\\cij -q*|| -\-Re, Rj, Oj) 

q6Bi{e) 

where Re is a scalar that satisfies ptot (IIq*T-Re II i Rj i n'j) = 
£. In the same way we obtain the second result. 
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Lemma 3 max{||V/3i||} < E ^ 

je{0-i} 1 

Proof 9 

vpi = ^Pj n 

je{0-i} 

The result follows since max{/3i} = 1 and by Lemma^ 
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